%% mazani
clc
clear
close all

umisteni='./../text/prace/img/';

%% first path
close all
xs=[0,5,10,15,45,100,105,110,115,120];
ys=[0,0,0,0,3.5,-2,-2,-2,-2,-2];

line=spline(xs,ys);
xs2=linspace(min(xs),max(xs),max(xs)+1);
ys2=ppval(line,xs2);
tau=linspace(0,1,length(xs2));
sp=spline(tau,[12,xs2,12;0,ys2,0]);
trajektorie=ppval(sp,linspace(0,1,10E2));

plot(trajektorie(1,:),trajektorie(2,:),'r');
grid on
xlabel('X [m]');
ylabel('Y [m]');
xlim([0,120]);

set(gcf,'PaperUnits', 'inches');
set(gcf,'PaperPosition', [0,0,3,3]);
set(gcf,'PaperSize',[3,3])

print([umisteni,'ref_path'],'-dpdf','-r300')

%% second path

close all
xs=[0,5,10,15,55,100,105,110,115,120];
ys=[0,0,0,0,3.5,-2,-2,-2,-2,-2];

line=spline(xs,ys);
xs2=linspace(min(xs),max(xs),max(xs)+1);
ys2=ppval(line,xs2);
tau=linspace(0,1,length(xs2));
sp=spline(tau,[12,xs2,12;0,ys2,0]);
trajektorie=ppval(sp,linspace(0,1,10E2));

plot(trajektorie(1,:),trajektorie(2,:),'r');
grid on
xlabel('X [m]');
ylabel('Y [m]');
xlim([0,120]);

set(gcf,'PaperUnits', 'inches');
set(gcf,'PaperPosition', [0,0,3,3]);
set(gcf,'PaperSize',[3,3])

print([umisteni,'ref_path2'],'-dpdf','-r300')

%% third path

close all
xs=[0,5,10,15,55,100,105,110,115,120];
ys=-[0,0,0,0,3.5,-2,-2,-2,-2,-2];

line=spline(xs,ys);
xs2=linspace(min(xs),max(xs),max(xs)+1);
ys2=ppval(line,xs2);
tau=linspace(0,1,length(xs2));
sp=spline(tau,[12,xs2,12;0,ys2,0]);
trajektorie=ppval(sp,linspace(0,1,10E2));

plot(trajektorie(1,:),trajektorie(2,:),'r');
grid on
xlabel('X [m]');
ylabel('Y [m]');
xlim([0,120]);

set(gcf,'PaperUnits', 'inches');
set(gcf,'PaperPosition', [0,0,3,3]);
set(gcf,'PaperSize',[3,3])

print([umisteni,'ref_path3'],'-dpdf','-r300')

%% fourth path

close all
xs=[0,5,10,15,45,100,105,110,115,120,125,130,135,140];
ys=[0,0,0,0,0,30,30,30,30,30,30,30,30,30];

line=spline(xs,ys);
xs2=linspace(min(xs),max(xs),max(xs)+1);
ys2=ppval(line,xs2);
tau=linspace(0,1,length(xs2));
sp=spline(tau,[12,xs2,12;0,ys2,0]);
trajektorie=ppval(sp,linspace(0,1,10E2));

plot(trajektorie(1,:),trajektorie(2,:),'r');
grid on
xlabel('X [m]');
ylabel('Y [m]');
xlim([0,120]);

set(gcf,'PaperUnits', 'inches');
set(gcf,'PaperPosition', [0,0,3,3]);
set(gcf,'PaperSize',[3,3])

print([umisteni,'ref_path4'],'-dpdf','-r300')

%% fifth path

close all
xs=[0,5,10,15,45,100,105,110,115,120,125,130,135,140];
ys=-[0,0,0,0,0,30,30,30,30,30,30,30,30,30];

line=spline(xs,ys);
xs2=linspace(min(xs),max(xs),max(xs)+1);
ys2=ppval(line,xs2);
tau=linspace(0,1,length(xs2));
sp=spline(tau,[12,xs2,12;0,ys2,0]);
trajektorie=ppval(sp,linspace(0,1,10E2));

plot(trajektorie(1,:),trajektorie(2,:),'r');
grid on
xlabel('X [m]');
ylabel('Y [m]');
xlim([0,120]);

set(gcf,'PaperUnits', 'inches');
set(gcf,'PaperPosition', [0,0,3,3]);
set(gcf,'PaperSize',[3,3])

print([umisteni,'ref_path5'],'-dpdf','-r300')
